#include <iostream>
#include <vector>
#include <random>
#include <cmath>
#include <ceres/ceres.h>

// 平面残差计算结构体（与之前相同）
struct PlaneResidual {
    PlaneResidual(double x, double y, double z) : x_(x), y_(y), z_(z) {}
    
    template <typename T>
    bool operator()(const T* const params, T* residuals) const {
        T z_predicted = params[0] * T(x_) + params[1] * T(y_) + params[2];
        residuals[0] = T(z_) - z_predicted;
        return true;
    }
    
    static ceres::CostFunction* Create(double x, double y, double z) {
        return new ceres::AutoDiffCostFunction<PlaneResidual, 1, 3>(
            new PlaneResidual(x, y, z)
        );
    }
    
private:
    const double x_, y_, z_;
};

// 弧度转角度的转换因子
const double RAD_TO_DEG = 180.0 / M_PI;

int main() {
    // 1. 生成20x20网格的模拟数据
    const int grid_size = 20;
    std::vector<double> x_data, y_data, z_data;
    
    // 真实平面参数（用于模拟倾斜）
    double true_a = 0.1;   // X方向斜率
    double true_b = 0.05;  // Y方向斜率
    double true_c = 5.0;   // 截距
    
    // 添加测量噪声
    std::default_random_engine generator;
    std::normal_distribution<double> noise(0.0, 0.05);  // 均值0，标准差0.05
    
    for (int i = 0; i < grid_size; ++i) {
        for (int j = 0; j < grid_size; ++j) {
            double x = j;  // X坐标（0~19mm）
            double y = i;  // Y坐标（0~19mm）
            double z = true_a * x + true_b * y + true_c + noise(generator);
            x_data.push_back(x);
            y_data.push_back(y);
            z_data.push_back(z);
        }
    }
    
    // 2. 使用Ceres拟合平面，得到参数a, b, c
    double params[3] = {0.0, 0.0, 0.0};  // 初始猜测值 [a, b, c]
    ceres::Problem problem;
    for (int i = 0; i < x_data.size(); ++i) {
        problem.AddResidualBlock(
            PlaneResidual::Create(x_data[i], y_data[i], z_data[i]),
            nullptr,
            params
        );
    }
    
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = false;  // 不输出优化过程
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    
    // 提取拟合得到的平面参数
    double a = params[0];  // X方向斜率系数
    double b = params[1];  // Y方向斜率系数
    
    // 3. 计算四个方向的倾斜角
    // X方向（沿x轴，y不变）
    double tilt_x_rad = std::atan(a);
    double tilt_x_deg = tilt_x_rad * RAD_TO_DEG;
    
    // Y方向（沿y轴，x不变）
    double tilt_y_rad = std::atan(b);
    double tilt_y_deg = tilt_y_rad * RAD_TO_DEG;
    
    // 对角线1（x=y方向，如从(0,0)到(1,1)）
    double slope_diag1 = a + b;  // 方向向量(1,1)的斜率
    double tilt_diag1_rad = std::atan(slope_diag1);
    double tilt_diag1_deg = tilt_diag1_rad * RAD_TO_DEG;
    
    // 对角线2（x=-y方向，如从(0,1)到(1,0)）
    double slope_diag2 = a - b;  // 方向向量(1,-1)的斜率
    double tilt_diag2_rad = std::atan(slope_diag2);
    double tilt_diag2_deg = tilt_diag2_rad * RAD_TO_DEG;
    
    // 4. 输出结果
    std::cout << "=== 平面拟合参数 ===" << std::endl;
    std::cout << "平面方程: z = " << a << "x + " << b << "y + " << params[2] << std::endl;
    
    std::cout << "\n=== 各方向倾斜角度 ===" << std::endl;
    std::cout << "X方向倾斜角: " << tilt_x_deg << "°" << std::endl;
    std::cout << "Y方向倾斜角: " << tilt_y_deg << "°" << std::endl;
    std::cout << "对角线1（x=y）倾斜角: " << tilt_diag1_deg << "°" << std::endl;
    std::cout << "对角线2（x=-y）倾斜角: " << tilt_diag2_deg << "°" << std::endl;
    
    return 0;
}
    